#ifndef __PID_H
#define __PID_H

#include "stdint.h"
#include "stdlib.h"

#define pid_Data_Limit(Data, min, max)      (((Data) < (min)) ? (min) : (((Data) > (max)) ? (max) : (Data)))
#define pid_ABS(x)                          ((x) < 0 ? (-(x)) : (x))        

typedef enum 
{
    PID_MODE_POSITION = 0,
    PID_MODE_INCREMENTAL,
    PID_MODE_ADD
} PID_MODE_TypeDef;

typedef struct
{
    float kp;
    float ki;
    float kd;
    float iOutMax;
    float outMax;
} PID_ParamTypeDef;

// 为各个电机定义 PID 参数结构体
typedef struct
{
    PID_ParamTypeDef position;
    PID_ParamTypeDef speed;
} Motor_PID_Params;

// 声明各个电机的 PID 参数
extern Motor_PID_Params motor1_pid_params;
// 如果有其他电机，也可以继续定义
// extern Motor_PID_Params motor2_pid_params;

// 初始化 PID 参数
#define MOTOR1_PID_PARAMS { \
    .position = { .kp = 2.0f, .ki = 0.1f, .kd = 0.5f, .iOutMax = 1000.0f, .outMax = 2000.0f }, \
    .speed = { .kp = 1.5f, .ki = 0.05f, .kd = 0.3f, .iOutMax = 800.0f, .outMax = 1500.0f } \
}

typedef struct
{
    PID_MODE_TypeDef    mode;
    float               setPoint;
    float               set;
    float               err[3];
    float               err3[3];
    float               pOut;
    float               iOut;
    float               dOut;
    float               out;            
    float               iOutMax;
    float               outMax;
    PID_ParamTypeDef    param;

    float               kp;
    float               ki;
    float               kd;
} PID_HandleTypeDef;

// 添加 PID_InitTypeDef 类型定义
typedef struct
{
    PID_MODE_TypeDef    mode;
    float               kp;
    float               ki;
    float               kd;
    float               iOutMax;
    float               outMax;
} PID_InitTypeDef;

// 函数声明
void PID_Init(PID_HandleTypeDef *pid, PID_MODE_TypeDef mode);
float PID_Calc(PID_HandleTypeDef *pid, float feedback);
void PID_Clear(PID_HandleTypeDef *pid);
void PID_SetParam(PID_HandleTypeDef *pid, PID_ParamTypeDef param);
void PID_SetTarget(PID_HandleTypeDef *pid, float target);
float PID_calc(PID_HandleTypeDef *pid, float exp_data, float real_data);
void PID_InitParams(PID_HandleTypeDef *pid, float kp, float ki, float kd, float iOutMax, float outMax);

#endif /* __PID_H */
